// Include files.
#include "../Internal/NullDeleter.hpp"
#include "../Internal/FpkException.hpp"
#include "CanJaguarWrapper.hpp"

using namespace boost;

/**
 *  Constructor for CanJaguarWrapper.
 *  
 *  @param canJaguar A pointer to the CANJaguar to wrap.
 *  @param encoderKind The type of position or distance sensor used.
 *  @param canDeleteCanJaguar If the CanJaguarWrapper can delete the CANJaguar so long as it functions correctly and no shared pointer is kept.
 */
CanJaguarWrapper::CanJaguarWrapper(CANJaguar *canJaguar, CanJaguarWrapper::EncoderKind encoderKind, bool canDeleteCanJaguar)
{
	if (canDeleteCanJaguar)
		m_canJaguar = shared_ptr<CANJaguar>(canJaguar);
	else
		m_canJaguar = shared_ptr<CANJaguar>(canJaguar, NullDeleter());
	
	SetupReference(encoderKind);
}

/**
 *  Constructor for CanJaguarWrapper.
 *  
 *  @param canJaguar A reference to the CANJaguar to wrap.
 *  @param encoderKind The type of position or distance sensor used.
 */
CanJaguarWrapper::CanJaguarWrapper(CANJaguar &canJaguar, CanJaguarWrapper::EncoderKind encoderKind) :
	m_canJaguar(&canJaguar, NullDeleter())
{
	SetupReference(encoderKind);
}

/**
 *  Constructor for CanJaguarWrapper.
 *  
 *  @param canJaguar A shared pointer to the CANJaguar to wrap.
 *  @param encoderKind The type of position or distance sensor used.
 */
CanJaguarWrapper::CanJaguarWrapper(shared_ptr<CANJaguar> &canJaguar, CanJaguarWrapper::EncoderKind encoderKind) :
	m_canJaguar(canJaguar)
{
	SetupReference(encoderKind);
}

/**
 *  Constructor for CanJaguarWrapper.
 *  
 *  It is assumed that the CANJaguar uses a quadrature encoder.
 *  
 *  @param canJaguar A pointer to the CANJaguar to wrap.
 *  @param canDeleteCanJaguar If the CanJaguarWrapper can delete the CANJaguar so long as it functions correctly and no shared pointer is kept.
 */
CanJaguarWrapper::CanJaguarWrapper(CANJaguar *canJaguar, bool canDeleteCanJaguar)
{
	if (canDeleteCanJaguar)
		m_canJaguar = shared_ptr<CANJaguar>(canJaguar);
	else
		m_canJaguar = shared_ptr<CANJaguar>(canJaguar, NullDeleter());
	
	SetupReference(QuadEncoder);
}

/**
 *  Constructor for CanJaguarWrapper.
 *  
 *  It is assumed that the CANJaguar uses a quadrature encoder.
 *  
 *  @param canJaguar A reference to the CANJaguar to wrap.
 */
CanJaguarWrapper::CanJaguarWrapper(CANJaguar &canJaguar) :
	m_canJaguar(&canJaguar, NullDeleter())
{
	SetupReference(QuadEncoder);
}

/**
 *  Constructor for CanJaguarWrapper.
 *  
 *  It is assumed that the CANJaguar uses a quadrature encoder.
 *  
 *  @param canJaguar A shared pointer to the CANJaguar to wrap.
 */
CanJaguarWrapper::CanJaguarWrapper(shared_ptr<CANJaguar> &canJaguar) :
	m_canJaguar(canJaguar)
{
	SetupReference(QuadEncoder);
}

/**
 *  Set the value of the motor.
 *  
 *  What it sets depends on the currently set ControlMode.
 *  
 *  @param speed The target value. (Parameter name may be misleading.)
 *  @param syncGroup See CANJaguar::Set function documentation. (The default value of this parameter will work fine.)
 */
void CanJaguarWrapper::Set(float speed, UINT8 syncGroup)
{
	m_canJaguar->Set(speed, syncGroup);
}

/**
 *  Gets the target value of the motor set by the user.
 *  
 *  What it gets depends on the currently set ControlMode.
 *  
 *  @return The target value set by the user.
 */
float CanJaguarWrapper::Get()
{
	return m_canJaguar->Get();
}

/**
 *  Disable the motor.
 */
void CanJaguarWrapper::Disable()
{
	return m_canJaguar->Disable();
}

/**
 *  Get the current speed that the motor is running in units of rotations per minute (RPM).
 *  
 *  @return The current speed of the motor. 
 */
double CanJaguarWrapper::GetSpeed()
{
	return m_canJaguar->GetSpeed();
}

/**
 *  Get the current position in units of rotations that the motor has reached since motor was enabled.
 *  
 *  @return The current position of the motor. 
 */
double CanJaguarWrapper::GetPosition()
{
	return m_canJaguar->GetPosition();
}

/**
 *  Enable the motor.
 *  
 *  If the parameter initialEncoderPosition is passed, the current position should be treated as initialEncoderPosition. 
 *  
 *  @param initialEncoderPosition The initial position of the motor.
 */
void CanJaguarWrapper::Enable(double initialEncoderPosition)
{
	m_canJaguar->EnableControl(initialEncoderPosition);
}

/**
 *  Get the current ControlMode that the motor is in.
 *  
 *  @return The current ControlMode. 
 */
PidMotorController::ControlMode CanJaguarWrapper::GetControlMode()
{
	return ControlMode(m_canJaguar->GetControlMode());
}

/**
 *  Set the ControlMode.
 *  
 *  You will need to call Enable to actually change the control mode.
 *  
 *  @param mode The desired ControlMode. 
 */
void CanJaguarWrapper::SetControlMode(PidMotorController::ControlMode mode)
{
	m_canJaguar->ChangeControlMode(CANJaguar::ControlMode(mode));
}

/**
 *  Set the number of encoder codes per revolution.
 *  
 *  Specify the number of encoder "codes" it takes to complete a revolution.
 *  
 *  @param codesPerRevolution The number of encoder codes per revolution.
 */
void CanJaguarWrapper::SetEncodersCodesPerRevolution(UINT16 codesPerRevolution)
{
	m_canJaguar->ConfigEncoderCodesPerRev(codesPerRevolution);
}

/**
 *  Set the P, I, and D values.
 *  
 *  @param p The P value.
 *  @param i The I value.
 *  @param d The D value.
 */
void CanJaguarWrapper::SetPid(double p, double i, double d)
{
	m_canJaguar->SetPID(p, i, d);
}

/**
 *  Get the current P value
 *  
 *  @return The current P value.
 */
double CanJaguarWrapper::GetP()
{
	return m_canJaguar->GetP();
}

/**
 *  Get the current I value
 *  
 *  @return The current I value.
 */
double CanJaguarWrapper::GetI()
{
	return m_canJaguar->GetI();
}

/**
 *  Get the current D value
 *  
 *  @return The current D value.
 */
double CanJaguarWrapper::GetD()
{
	return m_canJaguar->GetD();
}

/**
 *  Set the sensor type used to sense speed.
 *  
 *  @param speedReference The sensor type to be used to sense speed.
 */
void CanJaguarWrapper::SetSpeedReference(CANJaguar::SpeedReference speedReference)
{
	m_canJaguar->SetSpeedReference(speedReference);
}

/**
 *  Set the sensor type used to sense position or distance.
 *  
 *  @param speedReference The sensor type to be used to sense position or distance.
 */
void CanJaguarWrapper::SetPositionReference(CANJaguar::PositionReference positionReference)
{
	m_canJaguar->SetPositionReference(positionReference);
}

/**
 *  Configure the CANJaguar speed and position sensor types based on the sensor type given.
 *  
 *  @param encoderKind The sensor type to be used to sense speed and position.
 */
void CanJaguarWrapper::SetupReference(CanJaguarWrapper::EncoderKind encoderKind)
{
	switch (encoderKind)
	{
		case Nothing:
			SetSpeedReference(CANJaguar::kSpeedRef_None);
			SetPositionReference(CANJaguar::kPosRef_None);
			break;
		case Encoder:
			SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
			SetPositionReference(CANJaguar::kPosRef_None);
			break;
		case InvertedEncoder:
			SetSpeedReference(CANJaguar::kSpeedRef_InvEncoder);
			SetPositionReference(CANJaguar::kPosRef_None);
			break;
		case QuadEncoder:
			SetSpeedReference(CANJaguar::kSpeedRef_QuadEncoder);
			SetPositionReference(CANJaguar::kPosRef_QuadEncoder);
			break;
		case Potentiometer:
			SetSpeedReference(CANJaguar::kSpeedRef_None);
			SetPositionReference(CANJaguar::kPosRef_Potentiometer);
			break;
		default:
			throw FpkException("Not a valid encoder kind.");
			break;
	}
}
